#include <termios.h>
#include <QDateTime>
#include <rclcpp/rclcpp.hpp>
#include "rms_pkg/msg/sensor_data.hpp"
#include "../include/modbus_rtu.hpp"
#include "../include/global_logger.h"
#include "../include/db/database_manager.h"

using namespace std::chrono_literals;

class SensorPublisher : public QObject, public rclcpp::Node {
    Q_OBJECT
public:
    SensorPublisher() : Node("sensor_hcd5014_publisher") {

        LOG_INFO(" SensorPublisher 初始化开始");

        // 声明参数
        this->declare_parameter<std::string>("serial_port", "/dev/ttyUSB0");
        this->declare_parameter<int>("baud_rate", 9600);
        this->declare_parameter<int>("device_address", 0x33);
        this->declare_parameter<int>("retry_interval", 5); // 重试间隔(秒)
        this->declare_parameter<int>("read_interval", 1);  // 读取间隔(秒)
        this->declare_parameter<std::string>("robot_id","robot1001");

        LOG_DEBUG(" 参数声明完成");

        // 获取参数
        this->get_parameter("serial_port", serial_port_);
        this->get_parameter("baud_rate", baud_rate_);
        this->get_parameter("device_address", device_address_);
        this->get_parameter("retry_interval", retry_interval_);
        this->get_parameter("read_interval", read_interval_);
        this->get_parameter("robot_id", robot_id_);

        LOG_INFO(" 获取参数完成: serial_port={}, baud_rate={}, device_address={}, retry_interval={}s, read_interval={}s, robot_id={}",
                     serial_port_, baud_rate_, device_address_, retry_interval_, read_interval_, robot_id_);

        publisher_ = this->create_publisher<rms_pkg::msg::SensorData>("env_sensor_data", 10);
        LOG_INFO(" 创建传感器数据发布者");

        // 初始化数据库
        db_manager_ = new DatabaseManager(this);
        if (!db_manager_->initialize("rms_client.db")) {
            LOG_ERROR(" 数据库初始化失败: {}", db_manager_->lastError().toStdString());
        } else {
            // 创建传感器数据表
            QString tableDefinition = "id INTEGER PRIMARY KEY AUTOINCREMENT, "
                                     "robot_id TEXT, "
                                     "timestamp TEXT, "
                                     "air_temperature REAL, "
                                     "air_humidity REAL, "
                                     "light_intensity REAL, "
                                     "co2_concentration INTEGER, "
                                     "soil_temperature REAL, "
                                     "soil_moisture REAL";
            if (!db_manager_->createTable("sensor_data", tableDefinition)) {
                LOG_ERROR(" 创建传感器数据表失败: {}", db_manager_->lastError().toStdString());
            }
        }

        // 初始化连接
        initializeConnection();

        // 创建定时器处理重连和数据读取
        timer_ = this->create_wall_timer(
            std::chrono::seconds(read_interval_),
            [this]() { this->timer_callback(); });
        LOG_INFO("SensorPublisher 初始化完成！创建定时器，间隔: {}秒！", read_interval_);
    }

private:
    void initializeConnection() {
        LOG_DEBUG(" 开始初始化连接");

        // 将波特率转换为termios常量
        speed_t baud_rate_constant = getBaudRateConstant(baud_rate_);
        if (baud_rate_constant == 0) {
            LOG_ERROR(" 不支持的波特率: {}", baud_rate_);
            connection_status_ = -1; // 不支持的波特率
            return;
        }

        LOG_DEBUG(" 波特率转换完成: {}", baud_rate_);

        // 创建或重新创建Modbus连接
        modbus_ = std::make_unique<ModbusRTU>(serial_port_.c_str(), baud_rate_constant, device_address_);
        LOG_DEBUG(" 创建Modbus连接对象: port={}, baud_rate={}, device_address={}",
                      serial_port_, baud_rate_, device_address_);

        if (!modbus_->connect()) {
            LOG_WARN(" 无法连接到 Modbus 设备: {}", serial_port_);
            connection_status_ = 0; // 连接失败
            return;
        }

        LOG_INFO(" 成功连接到 Modbus 设备: {}", serial_port_);
        connection_status_ = 1; // 连接成功
        retry_count_ = 0;
    }

    void timer_callback() {
        // 如果没有连接成功，尝试重连
        if (connection_status_ != 1) {
            retry_count_++;
            if (retry_count_ % retry_interval_ == 0) { // 每隔retry_interval秒尝试重连一次
                LOG_INFO(" 尝试重新连接到 Modbus 设备 (第{}次尝试)", retry_count_);
                initializeConnection();
            }
            return;
        }

        // 已连接，读取数据
        std::vector<uint16_t> values;
        if (!modbus_->readHoldingRegisters(0x0001, 6, values)) {
            LOG_WARN(" 读取传感器数据失败");
            // 可以在这里添加连接状态检查逻辑
            return;
        }

        if (values.size() != 6) {
            LOG_WARN(" 读取到的数据长度不正确: expected=6, actual={}", values.size());
            return;
        }

        constexpr float SCALE_100 = 100.0f;
        //constexpr float SCALE_10 = 10.0f;

        auto msg = rms_pkg::msg::SensorData();
        msg.robot_id = robot_id_;
        msg.air_temperature = static_cast<float>(values[0]) / SCALE_100 - 40.0f;    //空气温度数据
        msg.air_humidity = static_cast<float>(values[1]) / SCALE_100;               //空气湿度数据
        msg.light_intensity = static_cast<float>(values[2]) / SCALE_100;            //光照强度数据
        msg.co2_concentration = values[3];                                          //CO2数据
        #if 0
        msg.soil_temperature = static_cast<float>(values[4]) / SCALE_100 - 20.0f;   //土壤温度数据(空)
        msg.soil_moisture = static_cast<float>(values[5]) / SCALE_10;               //土壤湿度数据(空)
        #else
        msg.soil_temperature = 0;   //土壤温度数据(空)
        msg.soil_moisture = 0;      //土壤湿度数据(空)
        #endif

        publisher_->publish(msg);

        // 保存到数据库
        saveToDatabase(msg);

        LOG_DEBUG(" 发布传感器数据: air_temperature={:.2f}℃, air_humidity={:.2f}%, "
                      "light_intensity={:.2f}lux, co2={}ppm, soil_temperature={:.2f}℃, "
                      "soil_moisture={:.1f}%",
                      msg.air_temperature, msg.air_humidity, msg.light_intensity,
                      msg.co2_concentration, msg.soil_temperature, msg.soil_moisture);
    }

    void saveToDatabase(const rms_pkg::msg::SensorData& msg) {
        if (!db_manager_ || !db_manager_->tableExists("sensor_data")) {
            LOG_WARN(" 数据库或表未初始化，无法保存数据");
            return;
        }

        QVariantMap data;
        data["robot_id"] = QString::fromStdString(msg.robot_id);
        data["timestamp"] = QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss.zzz");
        data["air_temperature"] = msg.air_temperature;
        data["air_humidity"] = msg.air_humidity;
        data["light_intensity"] = msg.light_intensity;
        data["co2_concentration"] = msg.co2_concentration;
        data["soil_temperature"] = msg.soil_temperature;
        data["soil_moisture"] = msg.soil_moisture;
        
        //无primaryKey主键参数则每次则插入一条新数据到数据库中
        if (!db_manager_->insertOrUpdate("sensor_data", data)) {
            LOG_ERROR(" 保存传感器数据到数据库失败: {}", db_manager_->lastError().toStdString());
        }
    }

    speed_t getBaudRateConstant(int baud_rate) {
        LOG_DEBUG(" 转换波特率常量: {}", baud_rate);

        switch (baud_rate) {
            case 9600:  LOG_DEBUG(" 波特率匹配: B9600"); return B9600;
            case 19200: LOG_DEBUG(" 波特率匹配: B19200"); return B19200;
            case 38400: LOG_DEBUG(" 波特率匹配: B38400"); return B38400;
            case 57600: LOG_DEBUG(" 波特率匹配: B57600"); return B57600;
            case 115200:LOG_DEBUG(" 波特率匹配: B115200"); return B115200;
            default:    LOG_WARN(" 不支持的波特率: {}", baud_rate); return 0;
        }
    }

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<rms_pkg::msg::SensorData>::SharedPtr publisher_;
    std::unique_ptr<ModbusRTU> modbus_;
    DatabaseManager* db_manager_;   // 数据库管理器

    // 参数变量
    std::string serial_port_;   // 串口设备路径
    int baud_rate_;             // 波特率
    int device_address_;        // 设备地址
    int retry_interval_;        // 重连间隔(秒)
    int read_interval_;         // 读取间隔(秒)
    std::string robot_id_;      // 添加robot_id变量

    // 连接状态管理
    int connection_status_ = 0; // 0=未连接, 1=已连接, -1=错误配置
    int retry_count_ = 0;

};
#include "sensor_hcd5014_publisher.moc"
int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    // 初始化全局日志记录器（如果尚未初始化）
    GlobalLogger::getInstance().initialize("data/logs", "RMSClient_hcd5014", 5);

    auto node = std::make_shared<SensorPublisher>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}